/*
 * Tracker.cpp
 *
 *  Created on: Sep 30, 2014
 *      Author: amorim
 */

#include <iostream>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "vision.h"



using namespace cv;
using namespace std;

TheEye::TheEye() {

	path = "/home/pi/workspace/opencv-cam/projects/opencv-cam/samples/";
	name = "snapshot.jpg";
	eye.connect();
	//eye.set_monochrome();
	eye.snapshot(path + name);

	Mat img = imread(path + name);
	if (!img.data) {
		cout << "Error! Could not load default image! \n";
	} else {
		set_current_img(img);
	}
}

void TheEye::edge_detection(Mat src, Mat dest, double lowThreshold) {

	// typical values of threshold: 10, 20, 50, 100
	Mat src_gray;
	Mat detected_edges;
	cvtColor(src, src_gray, CV_BGR2GRAY);

//	dest.create(src.size(), src.type());
	blur(src_gray, detected_edges, Size(3, 3));

	/// Canny detector
	Canny(detected_edges, detected_edges, lowThreshold, lowThreshold * 2, 3);

	src.copyTo(dest, detected_edges);
}

int TheEye::flood_fill(Mat dest, int x, int y) {

	int lo = 40, up = 100;

	Point seed = Point(x, y);
	int flags = CV_FLOODFILL_FIXED_RANGE;

	Rect ccomp;

	//Scalar newVal  = dest.at<Scalar>(seed);
	Scalar newVal = Scalar(R, G, B);
	int area;

	area = floodFill(dest, seed, newVal, &ccomp, Scalar(lo, lo, lo),
			Scalar(up, up, up), flags);

	circle(dest, seed, 5, Scalar(0, 0, 255), -1, 8);

	return area;

}

int TheEye::free_space(Mat src, Mat flooded) {

	flood_fill(flooded, EYE_X(flooded), EYE_Y(flooded));

	Vec3b floor_color = Vec3b(R, G, B);
	int size = 0, i = 0;
	int counting = 0;

	for (i = EYE_Y(flooded); i >= 0; i--) {

		Point seed = Point(EYE_X(flooded), i);
		Vec3b val = flooded.at<Vec3b>(seed);
		//cout << "Val= " << val << " Floor="<< floor_color << "\n";
		if (val == floor_color) {

			if (counting) {
				size++;
			} else {
				counting = 1;
			}
		} else {
			if (counting == 1) { // já estava contando e parou
				counting = 0;
				//return size; // not connected area
				break;
			}
			counting = 0;
		}

	}

	char buf[100];
	sprintf(buf, "free-space = %d", size);
	Point org = Point(EYE_X(flooded) + 15, EYE_Y(flooded));
	putText(flooded, buf, org, CV_FONT_HERSHEY_SIMPLEX, 0.5,
			Scalar(255, 50, 50), 1, false);

	return size;
}


void TheEye::set_current_img(Mat src) {
	currentImg.create(src.size(), src.type());
	src.copyTo(currentImg);
}
Mat TheEye::get_current_img() {
	return currentImg;
}

void TheEye::update() {
	eye.snapshot(path + name);
	Mat img = imread(path + name);
	if (!img.data) {
		cout << "Error! Could not load default image! \n";
	} else {
		set_current_img(img);
	}
}




//int iLowH = 170;
//int iHighH = 179;
//
//int iLowS = 150;
//int iHighS = 255;
//
//int iLowV = 60;
//int iHighV = 255;
//
//int iLastX = -1;
//int iLastY = -1;
//
//int track_red_ball(Mat imgOriginal, Mat imgLines) {
//
//	Mat imgHSV;
//	Mat imgThresholded;
//
//	cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV
//
//	inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV),
//			imgThresholded); //Threshold the image
//
//	//morphological opening (removes small objects from the foreground)
//	erode(imgThresholded, imgThresholded,
//			getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
//	dilate(imgThresholded, imgThresholded,
//			getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
//
//	//morphological closing (removes small holes from the foreground)
//	dilate(imgThresholded, imgThresholded,
//			getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
//	erode(imgThresholded, imgThresholded,
//			getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
//
//	//Calculate the moments of the thresholded image
//	Moments oMoments = moments(imgThresholded);
//
//	double dM01 = oMoments.m01;
//	double dM10 = oMoments.m10;
//	double dArea = oMoments.m00;
//
//	// if the area <= 10000, I consider that the there are no object in the image and it's because of the noise, the area is not zero
//	if (dArea > 10000) {
//		//calculate the position of the ball
//		int posX = dM10 / dArea;
//		int posY = dM01 / dArea;
//
//		if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) {
//			//Draw a red line from the previous point to the current point
//			line(imgLines, Point(posX, posY), Point(iLastX, iLastY),
//					Scalar(0, 0, 255), 2);
//		}
//
//		iLastX = posX;
//		iLastY = posY;
//	}
//
//	imgOriginal = imgOriginal + imgLines;
//
//	imshow("Thresholded", imgThresholded); //show the thresholded image
//
//	return 0;
//}

